#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>
#include<opencv2/opencv.hpp>

//#include <pcl/conversions.h>

#include <map>


using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_;
ros::Publisher cloud_pub;

void cloudCB(const sensor_msgs::ImageConstPtr& left_image_msg, const sensor_msgs::ImageConstPtr& right_image_msg,const sensor_msgs::PointCloud2ConstPtr& msg){
    ROS_INFO("cb ok");
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    cv_bridge::CvImagePtr image_left,image_right;
//    cv::Mat image;
//    cv_bridge::toCvShare(left_image_msg,"rgb8")->image;
    image_left = cv_bridge::toCvCopy(left_image_msg, sensor_msgs::image_encodings::BGR8);
    image_right = cv_bridge::toCvCopy(right_image_msg, sensor_msgs::image_encodings::BGR8);
//    cout<<"image size"<<(*depth_ptr).image.size()<<endl;
    cv::imshow("Show RgbImage", (*image_left).image);
    cv::imwrite("/media/st/Application/Ubuntu/dataset/gazebo/left/"+frame_id_string_+".png",(*image_left).image);
    cv::imwrite("/media/st/Application/Ubuntu/dataset/gazebo/right/"+frame_id_string_+".png",(*image_right).image);


    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*msg, *pc_tmp);

    pcl::io::savePCDFileASCII ("/media/st/Application/Ubuntu/dataset/gazebo/pointcloud/"+frame_id_string_+".pcd",*pc_tmp);


    frame_num_++;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);

    cv::namedWindow("Show RgbImage");
    cv::startWindowThread();

    message_filters::Subscriber<sensor_msgs::Image> image_left_sub(nh, "/husky_master/camera/left/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_right_sub(nh, "/husky_master/camera/right/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/husky_master/husky_master/velodyne_points2", 1);
//    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_left_sub, image_right_sub,pc_sub, 10);
//    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image,sensor_msgs::PointCloud2> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_left_sub, image_right_sub,pc_sub);
    ROS_INFO("ok1");
    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    ROS_INFO("ok2");
    ros::spin();

    return 0;
}
